import rclpy
from rclpy.node import Node
from face_interfaces.srv import FaceDetect
import face_recognition
import cv2
# 获取功能包路径
from ament_index_python.packages import get_package_share_directory 
from cv_bridge import CvBridge
import time
import os
from rcl_interfaces.msg import SetParametersResult


class FaceDetectNode(Node):

    def __init__(self):
        super().__init__("face_detect_node")
        self.service_=self.create_service(FaceDetect,"face_detect",self.detect_callback)
        
        self.bridge=CvBridge()
        # 设置参数
        self.declare_parameter('number_of_times_to_upsample',1)
        self.declare_parameter('model','hog')
        self.number_of_times_to_upsample=self.get_parameter('number_of_times_to_upsample').value
        self.model=self.get_parameter('model').value
        # 设置本地图片路径
        self.image_path=os.path.join(get_package_share_directory("demo_python_service")+"/img/OIP-C.jpeg")
        self.get_logger().info("人脸检测启动")

        # 参数回调函数
        self.add_on_set_parameters_callback(self.paramater_callbacke)

        # 设置自身节点参数的方法
        # self.set_parameters([rclpy.Parameter('model',rclpy.Parameter.Type.STRING,'hog')])


    def paramater_callbacke(self,paramaters):
        for paramater in paramaters:
            self.get_logger().info(f'{paramater.name}->{paramater.value}')
            if paramater.name=='number_of_times_to_upsample':
                self.number_of_times_to_upsample=paramater.value
            if paramater.name=='model':
                self.model=paramater.value
        
        return SetParametersResult(successful=True)


    def detect_callback(self,request,response):
        if request.image.data:
            cv_image=self.bridge.imgmsg_to_cv2(request.image)
        else:
            cv_image=cv2.imread(self.image_path)
        start_time=time.time()
        face_recognitions =face_recognition.face_locations(cv_image,number_of_times_to_upsample=self.number_of_times_to_upsample,model=self.model)
        end_time=time.time()

        # 数据填充
        response.use_time=end_time-start_time
        response.number=len(face_recognitions)
        for top,right,bottom,left in face_recognitions:
            response.top.append(top)
            response.right.append(right)
            response.bottom.append(bottom)
            response.left.append(left)
        
        return response


def main():
    rclpy.init()
    node=FaceDetectNode()
    rclpy.spin(node)
    rclpy.shutdown()
